/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "adc.h"
#include "can.h"
#include "dma.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "bsp_usart.h"
#include "bsp_can.h"
#include "motor.h"
#include "sbus.h"
#include "elerator_uart.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
#define PI     3.1416
#define wheel_radius 0.062 //轮子半径
#define gear_ratio   35.0    //减速比
#define LINEAR_TO_RPM_FACTOR ((gear_ratio * 60.0) / (2 * PI * wheel_radius))

//计算电池剩余电量百分比
#define Low_voltage  43.0
#define High_voltage  54.6

USER_CAN CAN_chassis(&hcan1,0),
				 CAN_communicate(&hcan2,1);
				 
MOTOR_SiFu Motor_left(0x581,&CAN_chassis),//Send_ID 0x601
					 Motor_right(0x582,&CAN_chassis),//Send_ID 0x602
					 Motor_rais(0x583,&CAN_chassis);

SBUS YK(&huart5);
MOTOR_DiPan DP;
SEND_DATA Send_Data;//上位机通讯数据
UpDown_check_class YK_s4(0);

uint8_t RxTemp_2 = 0;
uint8_t Flag=0;
int16_t YK_yingshe;
float Now_voltage,Vin,aaa;
uint16_t ADC_value[2];

uint8_t MRDH_Flag=1,Ds;
int32_t rais_target,
				rais_nowAngle; 
				
int8_t count_cs,RS_flag,RS_finished; 				

#define RISING   HAL_GPIO_ReadPin(GPIOD,GPIO_PIN_12)
#define LOWER   HAL_GPIO_ReadPin(GPIOD,GPIO_PIN_13)				
/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
int mapper(int val, int I_Min, int I_Max, int O_Min, int O_Max)
{
    return (((val - I_Min) * (O_Max - O_Min)) / (I_Max - I_Min)) + O_Min; // 整形映射函数
}
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
struct 	current_velStr			// 定义共用体，方便数据传输
{
	union
	{
		float f=0;
		unsigned char uc[4];
	} message1;
	union
	{
		float f=0;
		unsigned char uc[4];
	} message2;
	union
	{
		float f=0;
		unsigned char uc[4];
	} message3;
	union
	{
		float f=0;
		unsigned char uc[4];
	} message4;
} current_vel;

struct goal_velocityStr
{
	union
	{
		float f;
		unsigned char uc[4];
	} car_vx;
	union
	{
		float f;
		unsigned char uc[4];
	} car_vy;
	union
	{
		float f;
		unsigned char uc[4];
	} car_vw;
	union
	{
		float f;
		unsigned char uc[4];
	} car_deta;
} goal_velocity;
uint8_t uart3_recbuf[32],uart3_txbuf[32];
/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
	
	MX_USART3_UART_Init();
  MX_USART6_UART_Init();
  MX_CAN1_Init();
  MX_CAN2_Init();
  MX_TIM1_Init();
  MX_TIM2_Init();
  MX_UART5_Init();
  MX_TIM3_Init();
  MX_TIM14_Init();
  MX_UART4_Init();
  MX_ADC1_Init();
  /* USER CODE BEGIN 2 */
	CAN_chassis.Init(0,0);
	
	YK.Init();
	elevator_1.init();
	HAL_ADC_Start_DMA(&hadc1,(uint32_t*)ADC_value,2);
	HAL_TIM_Base_Start_IT(&htim1);
	HAL_TIM_Base_Start_IT(&htim2);
	HAL_TIM_Base_Start_IT(&htim3);//导航通信
	HAL_TIM_Base_Start_IT(&htim14);
	__HAL_UART_ENABLE_IT(&huart4, UART_IT_IDLE);
	HAL_UART_Receive_DMA(&huart4, uart3_recbuf,32);//minipc:460800
	HAL_Delay(500);
  /* USER CODE END 2 */
//	elevator_1.test();
//	elevator_2.test();
		//elevator_2.isNeedElevator=true;
		//elevator_1.isNeedElevator=true;
  /* Infinite loop */
	
  /* USER CODE BEGIN WHILE */
  while (1)
  {
		//INFO("rs:--  %d  \n",RISING);
			//elevator_1.test();
			elevator_1.controlElevator();
			elevator_2.controlElevator();
			if(MRDH_Flag==1||YK.failsafe_status == SBUS_SIGNAL_LOST){
				//INFO("lower:--  %d  \n",LOWER);
			
			if(Ds<=35)
			{
				if(Ds<=20){
					CAN_chassis.ClearError(0x601);
					CAN_chassis.set_ControlMode(0x601,3);
					CAN_chassis.Motor_Disable(0x601);
					HAL_Delay(1);
					CAN_chassis.ClearError(0x602);
					CAN_chassis.set_ControlMode(0x602,3);
					CAN_chassis.Motor_Disable(0x602);		
					HAL_Delay(1);
					CAN_chassis.ClearError(0x603);
					CAN_chassis.set_ControlMode(0x603,3);
					CAN_chassis.Motor_Disable(0x603);	
					HAL_Delay(1);					
				}else{
			
					CAN_chassis.set_ControlMode(0x601,3);
					CAN_chassis.Motor_Enable(0x601);
					HAL_Delay(1);
					CAN_chassis.set_ControlMode(0x602,3);
					CAN_chassis.Motor_Enable(0x602);	
					HAL_Delay(1);
					CAN_chassis.set_ControlMode(0x603,3);
					CAN_chassis.Motor_Enable(0x603);
					HAL_Delay(1);
				}
					//enable_flag=1;
					HAL_GPIO_WritePin(GPIOD,GPIO_PIN_5,GPIO_PIN_RESET);	
					Ds++;
								
			}
			
			DP.Chassis_Data_Deal(goal_velocity.car_vx.f*(512.0*65536.0)/1875.0,goal_velocity.car_vy.f*(512.0*65536.0)/1875.0,70);
		
			DP.Motor_RISING_LOWER(RS_flag);
			
			if(YK.yaogan.s2==YK_SW_DOWN){ //800
			
				if(YK.failsafe_status == SBUS_SIGNAL_LOST)
					{						
						MRDH_Flag=1;
						Ds=0;						
					}
				else
						MRDH_Flag=0;				
				}		
			
		}	
		
					
		if(YK.yaogan.s4== YK_SW_DOWN && YK.failsafe_status != SBUS_SIGNAL_LOST){
			MRDH_Flag=0;
			DP.Chassis_Data_Deal(goal_velocity.car_vx.f*(512.0*65536.0)/1875.0,goal_velocity.car_vy.f*(512.0*65536.0)/1875.0,70);
			DP.Motor_RISING_LOWER(RS_flag);
			
		}else if(YK.yaogan.s4==YK_SW_MID&& YK.failsafe_status != SBUS_SIGNAL_LOST){		
			if(MRDH_Flag!=1){
			static uint32_t last_switch_time = 0;
			uint32_t current_time = HAL_GetTick();
			DP.Chassis_Data_Deal(YK.yaogan.ch2*6.5536,YK.yaogan.ch0*6.5536,YK_yingshe);
			if (YK.yaogan.s1 == YK_SW_UP) { 
			if ((current_time - last_switch_time > 500) ) {
					count_cs=1;
					last_switch_time = current_time;
				}
		} else if (YK.yaogan.s1 == YK_SW_DOWN) {  
			if ((current_time - last_switch_time > 500)) {
					count_cs=2;
					last_switch_time = current_time;
    }
	}
			DP.Motor_RISING_LOWER(count_cs);
			}
		}
		
		if((YK_s4.updata(YK.yaogan.s4==YK_SW_UP)==UpDown_check_rising&&Flag<=20)&& MRDH_Flag==0 && (YK.failsafe_status != SBUS_SIGNAL_LOST))
		{
			Flag++;
			CAN_chassis.ClearError(0x601);
			CAN_chassis.set_ControlMode(0x601,3);
			CAN_chassis.Motor_Disable(0x601);
			HAL_Delay(2);
			CAN_chassis.ClearError(0x602);
			CAN_chassis.set_ControlMode(0x602,3);
			CAN_chassis.Motor_Disable(0x602);
			HAL_Delay(2);
			
			CAN_chassis.ClearError(0x603);			
			CAN_chassis.set_ControlMode(0x603,3);
			CAN_chassis.Motor_Disable(0x603);
			HAL_Delay(2);
			
			HAL_GPIO_WritePin(GPIOD,GPIO_PIN_5,GPIO_PIN_SET);
		}
		if((YK_s4.updata(YK.yaogan.s4==YK_SW_MID)==UpDown_check_rising&&Flag>=10)&& MRDH_Flag==0 && (YK.failsafe_status != SBUS_SIGNAL_LOST))
		{
			Flag--;
			CAN_chassis.set_ControlMode(0x601,3);
			CAN_chassis.Motor_Enable(0x601);
			HAL_Delay(2);
			CAN_chassis.set_ControlMode(0x602,3);
			CAN_chassis.Motor_Enable(0x602);
			HAL_Delay(2);

			CAN_chassis.set_ControlMode(0x603,3);
			CAN_chassis.Motor_Enable(0x603);
			HAL_Delay(2);
			
			HAL_GPIO_WritePin(GPIOD,GPIO_PIN_5,GPIO_PIN_RESET);
		}
		if(RISING==0||LOWER==0){
			RS_finished=1;
		}else{
			RS_finished=0;
		}
	

		
//		INFO("%d  %.2f \n",Motor_left.voltage,Now_voltage);
//		INFO("%.2f  %.2f  %.2f\n",aaa,Vin,Now_voltage);
//			INFO("%.2f  %.2f  %.2f  %.2f %d\n",DP.ML.left,DP.ML.right,DP.ML.qz,DP.ML.qy,Motor_left.Encoder);
//		INFO("%d  %d  %d  %d\n",YK.yaogan.ch0,YK.yaogan.ch1,YK.yaogan.ch2,YK.yaogan.ch3);
//		INFO("%d  %d  %d  %d\n",YK.yaogan.s1,YK.yaogan.s2,YK.yaogan.s3,YK.yaogan.s4);
//		INFO("%d   %d  %d\n",Motor_left.angle,Motor_left.Encoder,Motor_left.status);
//		INFO("%d   %d  %d\n",Motor_right.angle,Motor_right.Encoder,Motor_right.status);
//		INFO("%.2f  %.2f  %.2f  %.2f\n",goal_velocity.car_vx.f,goal_velocity.car_vy.f,(goal_velocity.car_vx.f*(512.0*10000.0))/1875.0,goal_velocity.car_vx.f*LINEAR_TO_RPM_FACTOR);
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 4;
  RCC_OscInitStruct.PLL.PLLN = 168;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	if(CAN_chassis.Receive(&hcan1)==HAL_OK)
	{
		if(Motor_left.update()==HAL_OK)
		{
			Motor_left.speed = (float)((Motor_left.Encoder*2.0*PI*wheel_radius)/(gear_ratio*60));
		}
		if(Motor_right.update()==HAL_OK)
		{
			Motor_right.speed = (float)(Motor_right.Encoder*2.0*PI*wheel_radius)/(gear_ratio*60);
		}
		Send_Data.Sensor_Str.X_speed = (Motor_left.speed - Motor_right.speed)/2.0;
	}
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	if(htim->Instance == TIM1)//500hz
  {
		static uint8_t count=0;
		if(count==0)
		{
			CAN_chassis.Get_motorData(0x601,3);
			CAN_chassis.Get_motorData(0x602,3);
			CAN_chassis.Get_motorData(0x603,3);
			count=1;
		}
		else if(count==1)
		{
			CAN_chassis.Get_motorData(0x601,2);
			CAN_chassis.Get_motorData(0x602,2);
			CAN_chassis.Get_motorData(0x603,2);
			count=2;
		}
		else if(count==2)
		{
			CAN_chassis.Get_motorData(0x601,1);
			CAN_chassis.Get_motorData(0x602,1);
			CAN_chassis.Get_motorData(0x603,1);
			count=0;
		}
		Now_voltage = (Motor_left.voltage - Low_voltage)/(High_voltage - Low_voltage);
  }
	if(htim->Instance == TIM2)//500hz
	{
		YK.SBUS_Handle();
		YK_yingshe = mapper(YK.yaogan.ch8,0,1600,0,90);
		if(YK.yaogan.s4==YK_SW_UP &&(YK.failsafe_status == SBUS_SIGNAL_OK))
		{
			CAN_chassis.Send_speed(0x601,0);
			CAN_chassis.Send_speed(0x602,0);
		}else
		{
			CAN_chassis.Send_speed(0x601,DP.ML.left);//DP.ML.qz
			CAN_chassis.Send_speed(0x602,-DP.ML.right);//DP.ML.qy
		}
		
		
	}
	if(htim==&htim3)//200hz
	{
		current_vel.message1.f = Send_Data.Sensor_Str.X_speed;
		current_vel.message2.f = Now_voltage;
		uart3_txbuf[0] = 0x45;
		memcpy(uart3_txbuf+1,current_vel.message1.uc,4);
		memcpy(uart3_txbuf+1+4,current_vel.message2.uc,4);
		uart3_txbuf[9]=RS_finished;
		HAL_UART_Transmit(&huart4,uart3_txbuf,11,0xffff);
		
		if(uart3_txbuf[10]&0x03){
			uart3_txbuf[10]=0;
		}
		
//		INFO("%.2f\n",current_vel.message1.f);
	}
	
	if(htim==&htim14)//100hz
	{
		static uint8_t send_count;
		if(RISING==0&&DP.ML.qz>0||LOWER==0&&DP.ML.qz<0)
		{
			//DP.ML.qz=0;
			CAN_chassis.Send_speed(0x603,0);
			send_count=3;
		}
		
    if(YK.yaogan.s4==YK_SW_UP)
    {
      // 急停状态不发送控制指令
    }
    else
    {
      // 分时发送不同电机的控制指令
      if(send_count==0)
      {
				  if(YK.yaogan.s3==YK_SW_DOWN){
							if((RISING==1&&YK.yaogan.ch1>0)||(LOWER==1&&YK.yaogan.ch1<0))
								CAN_chassis.Send_speed(0x603,YK.yaogan.ch1/2);  // 速度控制模式
							send_count=1;
					}else{send_count=2;}

      }else if(send_count==2)
      {
				CAN_chassis.Send_speed(0x603,DP.ML.qz);
        send_count=3;
      }else if(send_count==3||send_count==1){
				CAN_chassis.Get_motorData(0x603,2);
				send_count=0;
			}
	}
}
}

void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)
{
	if(hadc==&hadc1)
	{
		Vin = (float)(ADC_value[1]/4096.0)*3.3;
		aaa = (float)(ADC_value[0]/4096.0)*3.3;
	}
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	if (huart == &huart5)
	{
		YK.SBUS_RxcallBack(RxTemp_2);
		HAL_UART_Receive_IT(&huart5, (uint8_t *)&RxTemp_2, 1);
	}
}
/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
